#pragma once
#include <highgui.h>
#include "SurfPath.h"

class SurfTracker : public BaseVideoManager
{
protected:
	FILE* _file;

	IplImage* _gray;
    CvMemStorage* _storage;
    CvSURFParams _params;
	int frameCount;

	vector<SurfPath*> _knownFeatures;

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	cv::Mat* CreateFeatureMatrix(CvSeq* seq)
	{
		int length = (int)(seq->elem_size/sizeof(float));
		cv::Mat* m = new cv::Mat(seq->total, length, CV_32F);

		CvSeqReader reader;
		float* ptr = m->ptr<float>(0);
		cvStartReadSeq( seq, &reader );
		for(int i = 0; i < seq->total; i++ )
		{
			const float* descriptor = (const float*)reader.ptr;
			CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
			memcpy(ptr, descriptor, length*sizeof(float));
			ptr += length;
		}

		return m;
	}
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	void debug_draw(IplImage *frame, vector<SurfPath*>* v)
	{
		for (vector<SurfPath*>::const_iterator it = v->begin(); it != v->end(); ++it)
		{			
			cvCircle(frame, (*it)->GetPoint(), 5, CV_RGB(0,255,0));
		}
	}
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	cv::Mat* CreateFeatureMatrix(vector<SurfPath*>* v)
	{
		int length = (*v)[0]->GetLength();
		cv::Mat* m = new cv::Mat(v->size(), length, CV_32F);
		
		float* dst = m->ptr<float>(0);
		int byteToCopy = length * sizeof(float);

		for (vector<SurfPath*>::const_iterator it = v->begin(); it != v->end(); ++it)
		{
			SurfPath* p = *it;
			memcpy(dst, p->GetDescriptor(), byteToCopy);
			dst += length;
		}

		return m;
	}

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

public:
	SurfTracker(const char* videoFilePath) : 
		BaseVideoManager(videoFilePath)
	{
		_storage = cvCreateMemStorage(0);
		_params = cvSURFParams(400, 1);

		//_file = fopen("output\\1\\history.log", "w"); 
	}

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	virtual void OnFirstFrame(FrameKey* key, IplImage* frame)
	{
		_gray = cvCreateImage(cvGetSize(frame), 8, 1);
		cvCvtColor(frame, _gray, CV_BGR2GRAY);

		CvSeq *keypoints = 0, *descriptors = 0;
		cvExtractSURF( _gray, 0, &keypoints, &descriptors, _storage, _params );

		// convert new features to cv::Mat
		int length = (int)(descriptors->elem_size/sizeof(float));
		cv::Mat* knownMat = new cv::Mat(descriptors->total, length, CV_32F);

		// copy descriptors
		CvSeqReader reader, keypointReader;
		float* ptr = knownMat->ptr<float>(0);
		cvStartReadSeq( descriptors, &reader );

		for(int i = 0; i < descriptors->total; i++ )
		{
			const float* descriptor = (const float*)reader.ptr;
			CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );

			CvSURFPoint* p = (CvSURFPoint*)cvGetSeqElem( keypoints, i );

			// copy to overall matrix
			memcpy(ptr, descriptor, length*sizeof(float));
			_knownFeatures.push_back(new SurfPath(key, p->pt.x, p->pt.y, ptr, length));

			ptr += length;
		}

		// DEBUG:
		debug_draw(frame, &_knownFeatures);
		cvShowImage("wnd", frame);

		delete knownMat;
	}
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	virtual void OnFrame(FrameKey* key, IplImage* frame)
	{
		cvCvtColor(frame, _gray, CV_BGR2GRAY);
		CvSeq *keypoints = 0, *descriptors = 0;

		DWORD t1 = GetTickCount();
		cvClearMemStorage(_storage);
		cvExtractSURF( _gray, 0, &keypoints, &descriptors, _storage, _params );
		DWORD extructionTime = GetTickCount() - t1;

		printf("found %i features\r\n", descriptors->total);

		// lookup known features
		cv::Mat* knownMat = CreateFeatureMatrix(&_knownFeatures);
		
		// convert new frame features to
		int length = (int)(descriptors->elem_size/sizeof(float));
		cv::Mat* features = CreateFeatureMatrix(descriptors);

		// find nearest neighbors using FLANN
		cv::Mat indices(descriptors->total, 2, CV_32S);
		cv::Mat dists(descriptors->total, 2, CV_32F);

		t1 = GetTickCount();
		cv::flann::Index flannIndex(*knownMat, cv::flann::KDTreeIndexParams(4));  // using 4 randomized kdtrees
		flannIndex.knnSearch(*features, indices, dists, 2, cv::flann::SearchParams(64) ); // maximum number of leafs checked	
		DWORD searchTime = GetTickCount() - t1;
		
	    int* indices_ptr = indices.ptr<int>(0);
		float* dists_ptr = dists.ptr<float>(0);
		float* desData = features->ptr<float>(0);

		vector<SurfPath*> newFeatures;
		vector<SurfPath*> updatedFeatures;
		for (int i = 0; i < indices.rows; ++i) 
		{
			CvSURFPoint* p = (CvSURFPoint*)cvGetSeqElem( keypoints, i );				
			float* d = &desData[i * length];

			// if we got quality difference between neighbors
			if (dists_ptr[2*i] < 0.6*dists_ptr[2*i+1]) 
			{ 
				// we know that feature
				int knownFeatureIndex = indices_ptr[2*i];
				SurfPath* knownPatch = _knownFeatures[knownFeatureIndex];

				// disnacce check
				CvPoint lastPos = knownPatch->GetPoint();
				float dist = sqrt(pow (p->pt.x - lastPos.x, 2) + pow(p->pt.y - lastPos.y, 2) );
				if (dist < 50)
				{
					knownPatch->Update(key, p->pt.x, p->pt.y, d);
									
					updatedFeatures.push_back(knownPatch);
					//cvCircle(frame, knownPatch->GetPoint(), 5, CV_RGB(0,255,0));
				}
			}
			else
			{
				// new feature
				SurfPath* newPatch = new SurfPath(key, p->pt.x, p->pt.y, d, length);
				newFeatures.push_back(newPatch);

				//cvCircle(frame, newPatch->GetPoint(), 5, CV_RGB(0,0,255));
			}
		}

		// we update all founded features and collect new. now we must union known and new features sets
		for (vector<SurfPath*>::const_iterator it = newFeatures.begin(); it != newFeatures.end(); ++it)
			_knownFeatures.push_back(*it);

		// reduce time to life and bury almost dead
		int deleted = LivingPatch<SurfPath>::Lifecycle(&_knownFeatures, _file);
		
		// printing and drawing
		printf("[%i], extruction[%ims], searching: [%ims], extructed: %i, updated: %i, deleted %i\r\n",
				_knownFeatures.size(), extructionTime, searchTime, descriptors->total, updatedFeatures.size(), deleted);

		for (vector<SurfPath*>::const_iterator it = updatedFeatures.begin(); it != updatedFeatures.end(); ++it)
		{
			SurfPath* p = *it;
			p->DrawPath(frame, CV_RGB(255,255,255), CV_RGB(0,255,0));
		}		

		delete knownMat;
		delete features;

		cvShowImage("wnd", frame);

		// DEBUG:
		//char buff[100];
		//sprintf(buff, "output\\1\\%u.png", GetTickCount());
		//cvSaveImage(buff, frame);
	}

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	virtual ~SurfTracker()
	{
		cvReleaseImage(&_gray);
		cvReleaseMemStorage(&_storage);

		
		//for (vector<SurfPath*>::const_iterator it = _knownFeatures.begin(); it != _knownFeatures.end(); ++it)
		//	(*it)->Save(_file);
		//fclose(_file);

		BasePatch::ClearPatches(&_knownFeatures);
	}
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

};